#include "ac_pid.h"
#include <string.h>
#include <iostream>
#include <cmath>
#include <algorithm>




const double AC_PID_2PI =  3.141592653589793f * 2;


// Constructor
ACPid::ACPid(float initialP, float initialI, float initialD, float initialImax, float initialFiltHz, float dt) :
    m_dt(dt),
    m_integrator(0.0f),
    m_input(0.0f),
    m_derivative(0.0f)
{
    m_kp = initialP;
    m_ki = initialI;
    m_kd = initialD;
    m_imax = fabsf(initialImax);
    filtHz(initialFiltHz);

    // reset input filter to first value received
    m_flags.m_resetFilter = true;

    memset(&m_pidInfo, 0, sizeof(m_pidInfo));
}

// set_dt - set time step in seconds
void ACPid::setDt(float dt)
{
    // set dt and calculate the input filter alpha
    m_dt = dt;
}

// filt_hz - set input filter hz
void ACPid::filtHz(float hz)
{
    // sanity check _filt_hz
    m_filtHz = std::max(m_filtHz, AC_PID_FILT_HZ_MIN);
}


// set_input_filter_all - set input to PID controller
//  input is filtered before the PID controllers are run
//  this should be called before any other calls to get_p, get_i or get_d
void ACPid::setInputFilterAll(float input)
{
    // don't process inf or NaN
    if (!std::isfinite(input)) {
        return;
    }

    // reset input filter to value received
    if (m_flags.m_resetFilter) {
        m_flags.m_resetFilter = false;
        m_input = input;
        m_derivative = 0.0f;
    }

    // update filter and calculate derivative
    float input_filt_change = getFiltAlpha() * (input - m_input);
    m_input = m_input + input_filt_change;
    if (m_dt > 0.0f) {
        m_derivative = input_filt_change / m_dt;
    }
}

// set_input_filter_d - set input to PID controller
//  only input to the D portion of the controller is filtered
//  this should be called before any other calls to get_p, get_i or get_d
void ACPid::setInputFilterD(float input)
{
    // don't process inf or NaN
    if (!std::isfinite(input)) {
        return;
    }

    // reset input filter to value received
    if (m_flags.m_resetFilter) {
        m_flags.m_resetFilter = false;
        m_derivative = 0.0f;
    }

    // update filter and calculate derivative
    if (m_dt > 0.0f) {
        float derivative = (input - m_input) / m_dt;
        m_derivative = m_derivative + getFiltAlpha() * (derivative-m_derivative);
    }

    m_input = input;
}

float ACPid::getP()
{
    m_pidInfo.P = (m_input * m_kp);
    return m_pidInfo.P;
}

float ACPid::getI()
{
    if(!iszero(m_ki) && !iszero(m_dt)) {
        m_integrator += ((float)m_input * m_ki) * m_dt;
        if (m_integrator < -m_imax) {
            m_integrator = -m_imax;
        } else if (m_integrator > m_imax) {
            m_integrator = m_imax;
        }
        m_pidInfo.I = m_integrator;
        return m_integrator;
    }
    return 0;
}

float ACPid::getD()
{
    // derivative component
    m_pidInfo.D = (m_kd * m_derivative);
    return m_pidInfo.D;
}

float ACPid::getFf(float requested_rate)
{
    m_pidInfo.FF = (float)requested_rate * m_ff;
    return m_pidInfo.FF;
}


float ACPid::getPi()
{
    return getP() + getI();
}

float ACPid::getPid()
{
    return getP() + getI() + getD();
}

void ACPid::resetI()
{
    m_integrator = 0;
}


/// Overload the function call operator to permit easy initialisation
void ACPid::operator() (float p, float i, float d, float imaxval, float inputFiltHz, float dt)
{
    m_kp = p;
    m_ki = i;
    m_kd = d;
    m_imax = fabsf(imaxval);
    m_filtHz = inputFiltHz;
    m_dt = dt;
}

// calc_filt_alpha - recalculate the input filter alpha
float ACPid::getFiltAlpha() const
{
    if (iszero(m_filtHz)) {
        return 1.0f;
    }

    // calculate alpha
    float rc = 1/(AC_PID_2PI*m_filtHz);
    return m_dt / (m_dt + rc);
}
